#include <TimerOne.h>

#define INH_A  2
#define IN_A   3
#define INH_B  4
#define IN_B   5
#define INH_C  6
#define IN_C   7

#define MAX_PWM_PIN 6
#define MAX_PWM_DUTY 100

const int INH[] = {INH_A, INH_B, INH_C};
const int IN[]  = {IN_A, IN_B, IN_C};


static struct {
  byte n;
  byte dutyCycle[MAX_PWM_PIN];
  byte count[MAX_PWM_PIN];
  byte pin[MAX_PWM_PIN];
} pwmInfo;


void PWM_Init(int freq){
  // period = 1000*1000 us / freq / MAX_PWM_DUTY = 10000 / freq
  unsigned long period = 1000L*1000L / freq / MAX_PWM_DUTY ;  // us
  Timer1.initialize(period);
  Timer1.attachInterrupt(PWM_Accumulate);

  Serial.print("freq: "); Serial.println(freq);
  Serial.print("period: "); Serial.println(period);
  Serial.print("byte: "); Serial.println(sizeof(byte));
  Serial.print("int: "); Serial.println(sizeof(int));
  Serial.print("long: "); Serial.println(sizeof(long));
}


static void PWM_Accumulate(){
  for (int i=0; i < pwmInfo.n; i++) {
    (pwmInfo.count[i])++;
  }
}


void PWM_Toggle(){
  for (int i=0; i < pwmInfo.n; i++) {
    if (pwmInfo.count[i] == pwmInfo.dutyCycle[i]){
      digitalWrite(pwmInfo.pin[i], LOW);
    }
    else if (MAX_PWM_DUTY == pwmInfo.count[i]) {
      pwmInfo.count[i] = 0;  // reset
      digitalWrite(pwmInfo.pin[i], HIGH);
    }
  }
}


void PWM_Set(byte pin, byte duty){
  pinMode(pin, OUTPUT);
  // check repetition
  for (int i=0; i<pwmInfo.n; i++){
    if (pin == pwmInfo.pin[i]){
      pwmInfo.dutyCycle[i] = duty;
    }
  }

  // when no repeat
  if (pwmInfo.n < MAX_PWM_PIN) {
    pwmInfo.pin[pwmInfo.n] = pin;
    pwmInfo.dutyCycle[pwmInfo.n] = duty;
    pwmInfo.n ++;
  }
}


void setup(){
    Serial.begin(9600);
    Serial.println("Start");

    PWM_Init(400);
    PWM_Set(INH_A, 50);
    PWM_Set(INH_B, 50);
    PWM_Set(INH_C, 50);
}


void loop() {
  PWM_Toggle();
}
